t = 2
send = 1
# t1
lower1 = [50, 100, 80]
upper1 = [90, 170, 140]
# t2
#lower2 = [50, 100, 80]   #紫
#upper2 = [90, 170, 140]
#lower2 = [50, 100, 80]   #红
#upper2 = [90, 170, 140]
#lower2 = [50, 100, 80]   #黄
#upper2 = [90, 170, 140]
#lower2 = [60, 110, 106]   #绿
#upper2 = [90, 255, 255]
lower2 = [90, 120, 100]   #蓝
upper2 = [110, 210, 190]

import cv2 as cv
import numpy as np
import pyrealsense2 as rs
import socket
import matplotlib.pyplot as plt
from PIL import Image
# import zxing  # 导入解析包
import pyzbar.pyzbar as pyzbar

# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()

# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))

found_rgb = False
for s in device.sensors:
    if s.get_info(rs.camera_info.name) == 'RGB Camera':
        found_rgb = True
        break
if not found_rgb:
    print("The demo requires Depth camera with Color sensor")
    exit(0)

if device_product_line == 'L500':
    config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
else:
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start streaming
pipeline.start(config)

ip_remote = '127.0.0.1'
#ip_remote = '192.168.123.245' # upboard IP
port_remote = 8887 # port
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # setup socket


def printline(str):
    global send
    print("\r", "                    ", end='')
    if send == 0:
        print("\r", 'notSend  ' + str, end='')
    else:
        print("\r", str, end='')


def t1():
    global send
    while(1):
        max_center = -1
        max_x = 0
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        if not color_frame:
            continue

        # Convert images to numpy arrays
        color_image = np.asanyarray(color_frame.get_data())
        hsvFrame = cv.cvtColor(color_image, cv.COLOR_BGR2HSV)

        # 61, 158, 145
        color_lower = np.array(lower1, np.uint8) 
        color_upper = np.array(upper1, np.uint8) 
        color_mask = cv.inRange(hsvFrame, color_lower, color_upper) 

        # Morphological Transform, Dilation 
        # for each color and bitwise_and operator 
        # between imageFrame and mask determines 
        # to detect only that particular color 
        kernal = np.ones((5, 5), "uint8") 

        color_mask = cv.dilate(color_mask, kernal) 
        res = cv.bitwise_and(color_image, color_image, mask = color_mask) 

        # Creating contour to track red color 
        contours, hierarchy = cv.findContours(color_mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)

        for pic, contour in enumerate(contours): 
            area = max(contours,key = cv.contourArea)
            x, y, w, h = cv.boundingRect(area)
            max_x = max(x, max_x)
            max_center = x + (int)(0.5 * w)
            cv.circle(color_image,(x + (int)(0.5 * w), y + (int)(0.5 * h)), 1, (0,0,255), -1)
            color_image = cv.rectangle(color_image, (x, y), 
                                    (x + w, y + h), 
                                    (0, 0, 255), 2)

        if send:
            data = str(max_center)
            udp_socket.sendto(data.encode('utf-8'), (ip_remote, port_remote))
            printline(data)

        # show image
        cv.imshow("Color Detection in Real-Time", color_image)

        # press Esc to stop
        k = cv.waitKey(5) & 0xFF
        if k == 27:
            break
        elif k == 8:
            send = not send


def t2():
    global send
    while(1):
        max_center = -1
        max_x = 0
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        if not color_frame:
            continue
        
        # Convert images to numpy arrays
        color_image = np.asanyarray(color_frame.get_data())
        hsvFrame = cv.cvtColor(color_image, cv.COLOR_BGR2HSV)
        frame = color_image
        gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
        
        color_lower = np.array(lower2, np.uint8) 
        color_upper = np.array(upper2, np.uint8) 
        color_mask = cv.inRange(hsvFrame, color_lower, color_upper) 

        kernal = np.ones((5, 5), "uint8") 

        color_mask = cv.dilate(color_mask, kernal) 
        res = cv.bitwise_and(color_image, color_image, mask = color_mask) 
        # Creating contour to track red color 
        contours, hierarchy = cv.findContours(color_mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)

        for pic, contour in enumerate(contours): 
            area = max(contours,key = cv.contourArea)
            x, y, w, h = cv.boundingRect(area)
            max_x = max(x, max_x)
            max_center = x + (int)(0.5 * w)
            cv.circle(color_image,(x + (int)(0.5 * w), y + (int)(0.5 * h)), 1, (0,0,255), -1)
            color_image = cv.rectangle(color_image, (x, y), 
                                    (x + w, y + h), 
                                    (0, 0, 255), 2)
        data = str(max_center)
        
        # Convert images to numpy arrays
        #frame = np.asanyarray(color_frame.get_data()) # color_image
        #frame = cv.cvtColor(color_image, cv.COLOR_BGR2HSV) # hsvFrame

        #gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)  # 转换为灰色通道
        # hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV)  # 转换为HSV空间

        #circles = cv.HoughCircles(gray,cv.HOUGH_GRADIENT,1,600,param1=100,param2=60,minRadius=5,maxRadius=300)  
        #if circles is not None:  # 如果识别出圆
        #    for circle in circles[0]:
        #        #  获取圆的坐标与半径
        #        x = int(circle[0])
        #        y = int(circle[1])
        #        r = int(circle[2])
        #        cv.circle(frame, (x, y), r, (0, 0, 255), 3)  # 标记圆
        #        cv.circle(frame, (x, y), 3, (255, 255, 0), -1)  # 标记圆心
        #        # text = 'x:  '+str(x)+' y:  '+str(y)
        #        # cv.putText(frame, text, (10, 30), font, 1, (0, 255, 0), 2, cv.LINE_AA, 0)  # 显示圆心位置
        #        data = str(x)
        #        printline(x)
        #else: #识别二维码
        #    img = Image.fromarray(gray)
        #    barcodes = pyzbar.decode(img)
        #    # for barcode in barcodes:
        #    #     bardata = barcode.data.decode("utf-8")
        #    # im.save('tmp.jpg')
        #    # zx = zxing.BarCodeReader()  # 调用zxing二维码读取包
        #    # zxdata = zx.decode('tmp.jpg')  # 图片解码
        #    # print(barcodes)
        #    if barcodes != []:
        #        done = 1
        #        qrcodedata = barcodes[0].data.decode("utf-8")
        #        if qrcodedata == 'get down':
        #            data = str(-10)
        #            printline('get down')
        #        elif qrcodedata == 'nod':
        #            data = str(-11)
        #            printline('nod')
        #        elif qrcodedata == 'turn left':  # 返回记录的内容
        #            data = str(-12)
        #            printline('turn left')
        #        elif qrcodedata == 'turn right':
        #            data = str(-13)
        #            printline('turn right')
        #    else:
        #        printline('--')
        
        img = Image.fromarray(gray)
        barcodes = pyzbar.decode(img)
        if barcodes != []:
            done = 1
            qrcodedata = barcodes[0].data.decode("utf-8")
            if qrcodedata == 'get down':
                data = str(-10)
                printline('get down')
            elif qrcodedata == 'nod':
                data = str(-11)
                printline('nod')
            elif qrcodedata == 'turn left':  # 返回记录的内容
                data = str(-12)
                printline('turn left')
            elif qrcodedata == 'turn right':
                data = str(-13)
                printline('turn right')
        
        if send:
            udp_socket.sendto(data.encode('utf-8'), (ip_remote, port_remote))

        cv.imshow('frame', frame)
        k = cv.waitKey(5) & 0xFF
        if k == 27:
            break
        elif k == 8:
            send = not send


if t==1:
    t1()
if t==2:
    t2()

cv.destroyAllWindows()
